#include "calculator.h"

#include <cmath>
#include <iostream>

#define D2R(d) ((d) * M_PI / 180)
#define R2D(r) ((r) * 180 / M_PI)

//#define LogPrintf(fmt, ...) ::printf(fmt "\n", ## __VA_ARGS__);
#define LogPrintf(fmt, ...)

const double kMilPerDegree = 2000 * M_PI / 360;

void Calculator::setEnv(const Env &e) {
    env_ = e;
    updateInnerData();
}

void Calculator::setBullet(const Bullet &b) {
    bullet_ = b;
    updateInnerData();
}

void Calculator::setTarget(const Target &t) {
    target_ = t;
    updateInnerData();
}

void Calculator::setSpeed(double v) {
    start_v_ = v;
    updateInnerData();
}

void Calculator::updateInnerData() {
    const double rho = 1.293 * env_.AP * (273.15 / (273.15 + env_.T));
    const double t_r = D2R(target_.angle);
    A_ = M_PI * bullet_.d * bullet_.d / 4;
    Rx_ = 0.5 * bullet_.Cd * A_ * rho; //! 空气阻力系数
    h_L_ = ::cos(t_r) * target_.distance;
    v_L_ = ::sin(t_r) * target_.distance;
    start_E_ = 0.5 * bullet_.m * start_v_ * start_v_;
}

bool Calculator::findBestFlyAngle(AngleResult &result) {

    result.start_E = start_E_;
    result.start_EpA = start_E_ / A_ / 10000;

    double precision = 1;
    double best_angle = target_.angle;  //! 最佳发射角度

    double max_h_distance = 0;   //! 上次飞行的有效水平距离
    double min_error = 10000;

    //! 采用逐渐逼近法
    while (true) {
        double this_angle = best_angle + precision;
        LogPrintf("== this_angle: %.6f, best_angle: %.6f ==", this_angle, best_angle);

        const double start_rad = D2R(this_angle);
        const double l = h_L_ / times_;

        double v_h = ::cos(start_rad) * start_v_; //! 水平速度
        double v_v = ::sin(start_rad) * start_v_; //! 垂直速度
        double acc_t = 0; //! 累积运行时长
        double acc_h = 0; //! 累积水平位移
        double acc_v = 0; //! 累积垂直位移
        double rad = 0;
        double v2 = 0;

        bool cant_reach = false;
        double low_v = 0;

        for (int i = 0; i < times_; ++i) {
            double t = l / v_h;  //! 计算经过当前片的所需时间

            v2 = v_h * v_h + v_v * v_v;   //! 总速度的平方
            double air_f = -Rx_ * v2;    //! 总空气阻力

            rad = ::atan(v_v / v_h); //! 飞行角度
            double air_f_h = ::cos(rad) * air_f;    //! 水平空气阻力
            double air_f_v = ::sin(rad) * air_f;    //! 垂直空气阻力

            double a_h = air_f_h / bullet_.m;           //! 水平加速度
            double a_v = air_f_v / bullet_.m - env_.G;  //! 垂直加速度

            double v_diff_h = a_h * t;  //! 水平速度变化
            double v_diff_v = a_v * t;  //! 垂直速度变化

            v_h += v_diff_h;
            v_v += v_diff_v;

            acc_t += t;
            acc_v += v_v * t;
            acc_h += l;
            low_v = v_L_ * i / times_;

            if (acc_v < low_v) { //! 低于最低高度了，没有必要再算下去了
                cant_reach = true;
                break;
            }
        }

        bool is_finished = false;   //! 是否结束计算
        bool is_succ = false;       //! 是否计算成功

        auto this_error = low_v - acc_v;

        if (cant_reach) {   //! 如果未到达
            LogPrintf("unreach, max_h_distance: %.6f, acc_h: %.6f", max_h_distance, acc_h);
            if (max_h_distance < acc_h) {    //! 本次比上次跑得远
                LogPrintf("distance father then last angle");
                best_angle = this_angle;
                min_error = this_error;
                max_h_distance = acc_h;

            } else if (max_h_distance == acc_h) { //! 如果飞行的距离一样，则比较高度误差
                LogPrintf("unreach, min_error: %.6f, this_error: %.6f", min_error, this_error);
                if (this_error < min_error) {   //! 本次的误差比上次的好
                    LogPrintf("error less then last angle");
                    best_angle = this_angle;
                    min_error = this_error;

                } else if (precision < 0.0001) {
                    is_finished = true;
                    is_succ = false;

                } else {
                    LogPrintf("precision /= 2");
                    precision /= 2;
                }
            } else {    //! 本次还不如上次远，说明已经到头了
                LogPrintf("distance near then last angle");
                if (precision < 0.0001) {
                    is_finished = true;
                    is_succ = false;

                } else {
                    LogPrintf("precision /= 2");
                    precision /= 2;
                }
            }

        } else {    //! 如果到达了目标水平距离
            LogPrintf("reached, min_error: %.6f, this_error: %.6f", min_error, this_error);
            //! 则要看看高度与目标高度的误差是否达到要求
            if (precision < 0.0001) {
                //! 达到精确要求
                is_finished = true;
                is_succ = true;

            } else {
                LogPrintf("precision /= 2");
                precision /= 2;
            }
        }

        if (is_finished) {
            result.start_angle = best_angle;
            result.relative_angle = best_angle - target_.angle;
            result.relative_mil = result.relative_angle * kMilPerDegree;
            result.tof = acc_t;
            result.v_h = v_h;
            result.v_v = v_v;
            result.v = ::sqrt(v2);
            result.final_angle = R2D(rad);
            result.E = 0.5 * bullet_.m * (v_v * v_v + v_h * v_h);
            result.percent = result.E * 100 / start_E_;
            result.error = min_error;
            result.EpA = result.E / A_ / 10000;
            result.max_h_distance = max_h_distance;

            return is_succ;
        }
    }
}

void Calculator::fly(double angle, double l) {
    const double start_rad = D2R(angle);

    ::printf("Env, G:%.3f, T:%.1f, AP:%.1f\n", env_.G, env_.T, env_.AP);
    ::printf("Ball, m:%.3f, d:%.3f, Cd:%.3f\n", bullet_.m, bullet_.d, bullet_.Cd);
    ::printf("A:%.3f, Rx:%.3f\n", A_, Rx_);
    ::printf("speed:%.3f, angle:%.3f\n", start_v_, angle);
    ::printf("l:%.2f\n", l);

    double v_h = ::cos(start_rad) * start_v_; //! 水平速度
    double v_v = ::sin(start_rad) * start_v_; //! 垂直速度
    double acc_t = 0; //! 累积运行时长
    double acc_v = 0; //! 累积坠落距离
    double acc_h = 0; //! 累积坠落距离

    auto pfile = fopen("out.csv", "w");

    if (pfile != nullptr) {
        ::fprintf(pfile, "   acc_h,    acc_v,    angle,        v,      v_h,      v_v,        E,      EpA,  percent,    air_f,  air_f_h,  air_f_v,      a_h,     a_v,  v_d_h,  v_d_v\n");
        while (true) {
            double t = l / v_h;  //! 计算经过当前片的所需时间

            double v2 = v_h * v_h + v_v * v_v;   //! 总速度的平方
            double v = sqrt(v2);
            double air_f = -Rx_ * v2;    //! 总空气阻力

            double rad = ::atan(v_v / v_h); //! 飞行角度
            double air_f_h = ::cos(rad) * air_f;    //! 水平空气阻力
            double air_f_v = ::sin(rad) * air_f;    //! 垂直空气阻力

            double a_h = air_f_h / bullet_.m; //! 水平加速度
            double a_v = air_f_v / bullet_.m - env_.G; //! 垂直加速度

            double v_diff_h = a_h * t;  //! 水平速度变化
            double v_diff_v = a_v * t;  //! 垂直速度变化

            acc_t += t;
            acc_v += v_v * t;
            acc_h += l;

            v_h += v_diff_h;
            v_v += v_diff_v;

            double E = 0.5 * bullet_.m * v2;
            double EpA = E / A_ / 10000;
            double percent = 100 * E / start_E_;
            double angle = R2D(rad);
            if (angle < -85)
                break;

            ::fprintf(pfile, "%8.3f, %8.3f, %8.3f, %8.3f, %8.3f, %8.3f, %8.3f, %8.3f, %8.2f, %8.3f, %8.3f, %8.3f, %8.3f, %.3f, %6.3f, %6.3f\n",
                      acc_h, acc_v, angle, v, v_h, v_v, E, EpA, percent, air_f, air_f_h, air_f_v, a_h, a_v, v_diff_h, v_diff_v);
        }

        ::fclose(pfile);
    }
}
